import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32

import tf_transformations
import threading
import time
import math
from rclpy.executors import MultiThreadedExecutor

from spacemit_audio import find_audio_card, find_playback_card
from agv_common_class import  NavigationClient, AngleSubscriber, OdomSubscriber, RotateRobot, AngleNavGoalPoseSend

# 音频
from spacemit_audio import play_wav_non_blocking, play_wav, play_audio_in_thread
from spacemit_audio import ASRModel, RecAudioThreadPipeLine
# asr_model = ASRModel()
from tools.agv import execute_command


record_device = find_audio_card()
play_device = f'plughw:{find_playback_card()},0'
max_time_record = 2

func_map_zh = {
    "过来": "come_on",
    "回去": "back"
}

def run_executor(executor):
    executor.spin()  # 运行 ROS 2 事件循环（不会阻塞主线程）


def main():
    rclpy.init()

    angle_sub_node = AngleSubscriber()  # 声源定位信息订阅

    nav_pub = AngleNavGoalPoseSend(angle_sub_node, invert=True)

    # 多线程执行的管理器
    executor = MultiThreadedExecutor()
    executor.add_node(angle_sub_node)
    executor.add_node(nav_pub)

    executor_thread = threading.Thread(target=run_executor, args=(executor,), daemon=True)
    executor_thread.start()

    try:
        # wav_file_path = "tools/feedback_voice/dengdaihuanxing.wav"
        # play_wav(wav_file_path, device=play_device)
        while rclpy.ok():
            
            
            print("等待唤醒...")
            # angle_sub_node.trigger_event.wait()
            # angle_sub_node.trigger_event.clear()
            an = input("请输入角度:")
            nav_pub.angle_sub = float(an)

            nav_pub.calc_target_point()
            
            # node_rotate.rotate_to_angle()  # 触发旋转

            nav_pub.publish_pose()

            # wav_file_path = "tools/feedback_voice/zhengzaiqianwang.wav"
            # play_wav(wav_file_path, device=play_device)

    except KeyboardInterrupt:
        print("程序终止")

    finally:
        node_rotate.stop_move()
        time.sleep(0.1)
        executor.shutdown()
        if rclpy.ok():  # 只有在 rclpy 仍然运行时才调用 shutdown
            rclpy.shutdown()

if __name__ == '__main__':
    main()
